#include "Path.h"
#include "General Utils.hpp"
#include "GameWorld.h"


///////////////////////////// Constructors \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
// The constructors of the Path class

// Constructor 1
// =============
// This constructor assumes the path is being created based on the world's map
// given this assumption, it can take direct positions instead of NavGraph
// node indices

Path::Path(Vector2D start, Vector2D end, Permissions perm)
{
	// Finds the closests nodes to each position 
	int startNode = FindClosestNodeToPosition(start);
	int endNode = FindClosestNodeToPosition(end);


	// Create the Path
	Graph_SearchAStar<NavGraph,Heuristic_Euclid> Searcher(*WorldMap->GetGraph(), startNode, endNode, perm);
	list<int> t_Path = Searcher.GetPathToTarget(); 

	list<int>::iterator PointA;
	list<int>::iterator PointB;

	for(PointA = t_Path.begin(), PointB = ++(t_Path.begin()); PointB != t_Path.end(); PointA++, PointB++)
	{
		m_EdgeList.push_back(PathEdge(WorldMap->GetGraph()->GetNode(*PointA).Pos(), WorldMap->GetGraph()->GetNode(*PointB).Pos()));
	}
		
		
	curEdge = m_EdgeList.begin();
	curWaypoint = m_EdgeList.front().Source();

}

Path::Path(NavGraph& graph, int start, int finish, bool noisy)
{
	if(!noisy)
	{
		Graph_SearchAStar<NavGraph,Heuristic_Euclid> Searcher(graph, start, finish);
		list<int> t_Path = Searcher.GetPathToTarget(); 

		list<int>::iterator PointA;
		list<int>::iterator PointB;

		for(PointA = t_Path.begin(), PointB = ++(t_Path.begin()); PointB != t_Path.end(); PointA++, PointB++)
		{
			m_EdgeList.push_back(PathEdge(graph.GetNode(*PointA).Pos(), graph.GetNode(*PointB).Pos()));
		}
		
		curWaypoint = m_EdgeList.front().Source();
	}

	else
	{
		Graph_SearchAStar<NavGraph,Heuristic_Noisy_Euclidian> Searcher(graph, start, finish);
		list<int> t_Path = Searcher.GetPathToTarget(); 

		list<int>::iterator PointA;
		list<int>::iterator PointB;

		for(PointA = t_Path.begin(), PointB = ++(t_Path.begin()); PointB != t_Path.end(); PointA++, PointB++)
		{
			m_EdgeList.push_back(PathEdge(graph.GetNode(*PointA).Pos(), graph.GetNode(*PointB).Pos()));
		}
		
		curEdge = m_EdgeList.begin();
		curWaypoint = curEdge->Source();
	}

}

Path::Path(const Path& CopyPath): m_EdgeList(CopyPath.m_EdgeList)
{
	curEdge = m_EdgeList.begin();
	curWaypoint = m_EdgeList.front().Source();
}

Path::Path(const list<PathEdge> edge_list): m_EdgeList(edge_list)
{
	if(!m_EdgeList.empty())
	{
		curEdge = m_EdgeList.begin();
		curWaypoint = curEdge->Source();
	}
}



void Path::Draw()
{

	list<PathEdge>::iterator EdgeIter = m_EdgeList.begin();

	while(EdgeIter != m_EdgeList.end())
	{
		Vector2D start = EdgeIter->Source();
		Vector2D end = EdgeIter->Destination();

		GameRectangle aRect = GetRect(start.x, start.y, end.x, end.y);
	
		if(ClipPlane->IsInPlane(aRect))
			DrawPrimitives->DrawLine(start.x - ClipPlane->GetX(), start.y - ClipPlane->GetY(),
			end.x - ClipPlane->GetX(), end.y - ClipPlane->GetY(), green);
		
		EdgeIter++;
	}


	//(The intersect function only takes rectangles, but I want to test for a single point)
	GameRectangle lastPoint;
	lastPoint.x = m_EdgeList.back().Destination().x;
	lastPoint.y = m_EdgeList.back().Destination().y;

	lastPoint.width = 0; lastPoint.height = 0; 
	if(ClipPlane->IsInPlane(lastPoint))
	{
		DrawPrimitives->DrawCircle(lastPoint.x - ClipPlane->GetX(), lastPoint.y - ClipPlane->GetY(), 10, red);
	}
}

// ================================= Find Closest Node To Position ===================================
// Only for use with paths that rely on the world map. Finds the node index closest to any given
// position Pos

int Path::FindClosestNodeToPosition(Vector2D Pos)
{
	if(Pos.x > WorldMap->MapWidth() * 32.0 || Pos.y > WorldMap->MapHeight() * 32.0 || Pos.x < 0 || Pos.y < 0)
		return -1;

	double NodeX = (Pos.x)/32.0;
	double NodeY = (Pos.y)/32.0;
	
	int nodeIndx = int(NodeY)*WorldMap->MapWidth() + int(NodeX);

	return nodeIndx;

}

list<Vector2D> Path::GetWaypoints()
{
	list<PathEdge>::iterator EdgeIter = m_EdgeList.begin();
	list<Vector2D> Waypoints;
	while(EdgeIter != m_EdgeList.end())
	{
		if(EdgeIter != (--m_EdgeList.end()))
			Waypoints.push_back(EdgeIter->Source());
		else
			Waypoints.push_back(EdgeIter->Destination());

		EdgeIter++;
	}

	return Waypoints;
}

